/*
 * @Author       : jiejie
 * @GitHub       : https://github.com/jiejieTop
 * @Date         : 2022-10-28 16:31:37
 * @LastEditors  : jiejie
 * @LastEditTime : 2022-12-01 14:42:55
 * @FilePath     : /middleware_benchmark/ecal/loc/loc_snd/src/loc_snd.cpp
 * Copyright (c) 2022 jiejie, All Rights Reserved. Please keep the author
 * information and source code according to the license.
 */

#include <ecal/ecal.h>
#include <ecal/msg/protobuf/publisher.h>
#include <tclap/CmdLine.h>

#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>

#include <algorithm>
#include <iostream>
#include <numeric>
#include <queue>

#include "loc.pb.h"
#include "ros_helper.h"

static size_t g_msg_num = 0;

// warmup runs not to measure
const int warmups(20);

std::vector<std::string> ros_sub_topics{ "/gps/fix",          "/gps/odom",          "/imu/data",
                                         "/localization/fix", "/localization/odom", "/location/outdoor/odom" };

std::vector<std::string> ecal_pub_topics{ "/ecal/gps/fix",           "/ecal/gps/odom",
                                          "/ecal/imu/data",          "/ecal/localization/fix",
                                          "/ecal/localization/odom", "/ecal/location/outdoor/odom" };

static inline void evaluate(std::vector<float>& arr)
{
    if (arr.size() >= warmups) {
        arr.erase(arr.begin(), arr.begin() + warmups);
    }

    std::stringstream ss;
    size_t sum_msg = arr.size();
    ss << "------------------------------------------------------" << std::endl;
    ss << "Messages send                           : " << sum_msg << std::endl;
    auto sum_time = std::accumulate(arr.begin(), arr.end(), 0.0f);
    auto avg_time = sum_time / (sum_msg * 1.0);
    auto min_it = std::min_element(arr.begin(), arr.end());
    auto max_it = std::max_element(arr.begin(), arr.end());
    size_t min_pos = min_it - arr.begin();
    size_t max_pos = max_it - arr.begin();
    auto min_time = *min_it;
    auto max_time = *max_it;
    ss << "ROS to eCAL image average latency       : " << avg_time << " ms" << std::endl;
    ss << "ROS to eCAL image min latency           : " << min_time << " ms @ " << min_pos << std::endl;
    ss << "ROS to eCAL image max latency           : " << max_time << " ms @ " << max_pos << std::endl;
    ss << "------------------------------------------------------" << std::endl;
    std::cout << ss.str();
}

static void ros_to_ecal_fix_send(std::queue<sensor_msgs::NavSatFix::ConstPtr>* ros_fix_queue,
                                 eCAL::protobuf::CPublisher<pb::NavSatFix>* ecal_fix_pub, int runs)
{
    // generate a class instance of Image
    std::vector<float> ros_to_ecal_latency_array;
    uint32_t seq = 0;
    while (1) {
        if (ros_fix_queue->empty()) {
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
            continue;
        }
        pb::NavSatFix fix;
        auto ros_fix = ros_fix_queue->front();

        seq++;
        auto start_time =
            std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch())
                .count();
        auto header = fix.mutable_header();
        header->set_seq(seq);
        header->set_timestamp(start_time);  // us
        if (ros_fix->header.frame_id != "") {
            header->set_frame_id(ros_fix->header.frame_id);
        }

        auto status = fix.mutable_status();
        status->set_status((pb::NavSatStatus_Status)ros_fix->status.status);

        fix.set_latitude(ros_fix->latitude);
        fix.set_longitude(ros_fix->longitude);
        fix.set_altitude(ros_fix->altitude);

        for (int i = 0; i < ros_fix->position_covariance.size(); i++) {
            fix.add_position_covariance(ros_fix->position_covariance[i]);
        }

        fix.set_position_covariance_type(ros_fix->position_covariance_type);

        fix.set_send_count(runs + warmups);

        auto end_time =
            std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch())
                .count();

        // 计算时间戳
        auto diff = (end_time - start_time) / 1000.0;

        fix.set_send_time(end_time);
        fix.set_convert_time(diff);

        // send the fix object
        ecal_fix_pub->Send(fix, end_time);

        ros_to_ecal_latency_array.push_back(diff);
        ros_fix_queue->pop();

        if (ros_to_ecal_latency_array.size() >= runs + warmups) {
            evaluate(ros_to_ecal_latency_array);
            g_msg_num += runs;
            return;
        }
    }
}

static void ros_to_ecal_odom_send(std::queue<nav_msgs::Odometry::ConstPtr>* ros_odom_queue,
                                  eCAL::protobuf::CPublisher<pb::Odometry>* ecal_odom_pub, int runs)
{
    // generate a class instance of Image

    std::vector<float> ros_to_ecal_latency_array;
    uint32_t seq = 0;
    while (1) {
        if (ros_odom_queue->empty()) {
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
            continue;
        }

        pb::Odometry odom;

        auto ros_odom = ros_odom_queue->front();

        seq++;
        auto start_time =
            std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch())
                .count();
        auto header = odom.mutable_header();
        header->set_seq(seq);
        header->set_timestamp(start_time);  // us

        if (ros_odom->header.frame_id != "") {
            header->set_frame_id(ros_odom->header.frame_id);
        }

        if (ros_odom->child_frame_id != "") {
            odom.set_child_frame_id(ros_odom->child_frame_id);
        }

        auto pose_with_covariance = odom.mutable_pose();
        auto pose = pose_with_covariance->mutable_pose();
        auto position = pose->mutable_position();
        position->set_x(ros_odom->pose.pose.position.x);
        position->set_y(ros_odom->pose.pose.position.y);
        position->set_z(ros_odom->pose.pose.position.z);

        auto orientation = pose->mutable_orientation();
        orientation->set_x(ros_odom->pose.pose.orientation.x);
        orientation->set_y(ros_odom->pose.pose.orientation.y);
        orientation->set_z(ros_odom->pose.pose.orientation.z);
        orientation->set_w(ros_odom->pose.pose.orientation.w);

        for (int i = 0; i < ros_odom->pose.covariance.size(); i++) {
            pose_with_covariance->add_covariance(ros_odom->pose.covariance[i]);
        }

        auto twist_with_covariance = odom.mutable_twist();
        auto twist = twist_with_covariance->mutable_twist();
        auto linear = twist->mutable_linear();
        linear->set_x(ros_odom->twist.twist.linear.x);
        linear->set_y(ros_odom->twist.twist.linear.y);
        linear->set_z(ros_odom->twist.twist.linear.z);

        auto angular = twist->mutable_angular();
        angular->set_x(ros_odom->twist.twist.angular.x);
        angular->set_y(ros_odom->twist.twist.angular.y);
        angular->set_z(ros_odom->twist.twist.angular.z);

        for (int i = 0; i < ros_odom->twist.covariance.size(); i++) {
            twist_with_covariance->add_covariance(ros_odom->twist.covariance[i]);
        }

        odom.set_send_count(runs + warmups);

        auto end_time =
            std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch())
                .count();

        // 计算时间戳
        auto diff = (end_time - start_time) / 1000.0;

        odom.set_send_time(end_time);
        odom.set_convert_time(diff);

        // send the odom object
        ecal_odom_pub->Send(odom, end_time);

        ros_to_ecal_latency_array.push_back(diff);
        ros_odom_queue->pop();

        if (ros_to_ecal_latency_array.size() >= runs + warmups) {
            evaluate(ros_to_ecal_latency_array);
            g_msg_num += runs;
            return;
        }
    }
}

static void ros_to_ecal_imu_send(std::queue<sensor_msgs::Imu::ConstPtr>* ros_imu_queue,
                                 eCAL::protobuf::CPublisher<pb::Imu>* ecal_imu_pub, int runs)
{
    // generate a class instance of Image

    std::vector<float> ros_to_ecal_latency_array;
    uint32_t seq = 0;
    while (1) {
        if (ros_imu_queue->empty()) {
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
            continue;
        }
        pb::Imu imu;
        auto ros_imu = ros_imu_queue->front();

        seq++;
        auto start_time =
            std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch())
                .count();
        auto header = imu.mutable_header();
        header->set_seq(seq);
        header->set_timestamp(start_time);  // us

        if (ros_imu->header.frame_id != "") {
            header->set_frame_id(ros_imu->header.frame_id);
        }

        auto orientation = imu.mutable_orientation();
        orientation->set_x(ros_imu->orientation.x);
        orientation->set_y(ros_imu->orientation.y);
        orientation->set_z(ros_imu->orientation.z);
        orientation->set_w(ros_imu->orientation.w);

        auto angular_velocity = imu.mutable_angular_velocity();
        angular_velocity->set_x(ros_imu->angular_velocity.x);
        angular_velocity->set_y(ros_imu->angular_velocity.y);
        angular_velocity->set_z(ros_imu->angular_velocity.z);

        auto linear_acceleration = imu.mutable_linear_acceleration();
        linear_acceleration->set_x(ros_imu->linear_acceleration.x);
        linear_acceleration->set_y(ros_imu->linear_acceleration.y);
        linear_acceleration->set_z(ros_imu->linear_acceleration.z);

        for (int i = 0; i < ros_imu->orientation_covariance.size(); i++) {
            imu.add_orientation_covariance(ros_imu->orientation_covariance[i]);
        }

        for (int i = 0; i < ros_imu->angular_velocity_covariance.size(); i++) {
            imu.add_angular_velocity_covariance(ros_imu->angular_velocity_covariance[i]);
        }

        for (int i = 0; i < ros_imu->linear_acceleration_covariance.size(); i++) {
            imu.add_linear_acceleration_covariance(ros_imu->linear_acceleration_covariance[i]);
        }

        imu.set_send_count(runs + warmups);

        auto end_time =
            std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch())
                .count();

        // 计算时间戳
        auto diff = (end_time - start_time) / 1000.0;

        imu.set_send_time(end_time);
        imu.set_convert_time(diff);

        // send the odom object
        ecal_imu_pub->Send(imu, end_time);

        ros_to_ecal_latency_array.push_back(diff);
        ros_imu_queue->pop();

        if (ros_to_ecal_latency_array.size() >= runs + warmups) {
            evaluate(ros_to_ecal_latency_array);
            g_msg_num += runs;
            return;
        }
    }
}

int main(int argc, char** argv)
{
    // initialize eCAL API
    eCAL::Initialize(argc, argv, "image_publisher");
    TCLAP::CmdLine cmd("image_publisher");

    // set process state
    eCAL::Process::SetState(proc_sev_healthy, proc_sev_level1, "I feel good !");

    TCLAP::ValueArg<int> runs("r", "runs", "Number of messages to send.", false, 1000, "int");
    TCLAP::ValueArg<int> ros_num("n", "number", "Number of ros sub.", false, 3, "int");
    TCLAP::ValueArg<int> mem_buffer("b", "mem_buffer", "Number of memory files per connection.", false, 1, "int");
    TCLAP::SwitchArg zero_copy("z", "zero_copy", "Switch zero copy mode on.");

    cmd.add(runs);
    cmd.add(ros_num);
    cmd.add(mem_buffer);
    cmd.add(zero_copy);
    cmd.parse(argc, argv);

    std::vector<std::thread> thread_vec;
    std::vector<eCAL::protobuf::CPublisher<pb::NavSatFix>*> ecal_fix_pub_vec;
    std::vector<eCAL::protobuf::CPublisher<pb::Odometry>*> ecal_odom_pub_vec;
    std::vector<eCAL::protobuf::CPublisher<pb::Imu>*> ecal_imu_pub_vec;

    std::map<std::string, std::queue<sensor_msgs::NavSatFix::ConstPtr>*> ros_fix_queue_map;
    std::map<std::string, std::queue<nav_msgs::Odometry::ConstPtr>*> ros_odom_queue_map;
    std::map<std::string, std::queue<sensor_msgs::Imu::ConstPtr>*> ros_imu_queue_map;

    eCAL::CPublisher stop_signal_pub("loc_stop_signal");

    for (int i = 0; i < ros_num.getValue(); i++) {
        if (i > ros_sub_topics.size()) {
            break;
        }
        // 判断 ros_sub_topics[i] 是否包含 fix odom imu
        if (ros_sub_topics[i].find("fix") != std::string::npos) {
            auto pub = new eCAL::protobuf::CPublisher<pb::NavSatFix>(ecal_pub_topics[i]);
            pub->ShmSetBufferCount(mem_buffer.getValue());
            pub->ShmEnableZeroCopy(zero_copy.getValue());
            ecal_fix_pub_vec.push_back(pub);
            auto queue = new std::queue<sensor_msgs::NavSatFix::ConstPtr>();
            ros_fix_queue_map[ros_sub_topics[i]] = queue;
            ros::ros_helper::instance().subscribe<sensor_msgs::NavSatFix>(
                ros_sub_topics[i], 10,
                (boost::function<void(const sensor_msgs::NavSatFix::ConstPtr&)>)[queue](
                    const sensor_msgs::NavSatFix::ConstPtr& msg) { queue->push(msg); });
            std::thread thread(ros_to_ecal_fix_send, std::ref(queue), std::ref(pub), runs.getValue());
            thread_vec.push_back(std::move(thread));
        } else if (ros_sub_topics[i].find("odom") != std::string::npos) {
            auto pub = new eCAL::protobuf::CPublisher<pb::Odometry>(ecal_pub_topics[i]);
            pub->ShmSetBufferCount(mem_buffer.getValue());
            pub->ShmEnableZeroCopy(zero_copy.getValue());
            ecal_odom_pub_vec.push_back(pub);
            auto queue = new std::queue<nav_msgs::Odometry::ConstPtr>();
            ros_odom_queue_map[ros_sub_topics[i]] = queue;
            ros::ros_helper::instance().subscribe<nav_msgs::Odometry>(
                ros_sub_topics[i], 10,
                (boost::function<void(const nav_msgs::Odometry::ConstPtr&)>)[queue](
                    const nav_msgs::Odometry::ConstPtr& msg) { queue->push(msg); });
            std::thread thread(ros_to_ecal_odom_send, std::ref(queue), std::ref(pub), runs.getValue());
            thread_vec.push_back(std::move(thread));
        } else if (ros_sub_topics[i].find("imu") != std::string::npos) {
            auto pub = new eCAL::protobuf::CPublisher<pb::Imu>(ecal_pub_topics[i]);
            pub->ShmSetBufferCount(mem_buffer.getValue());
            pub->ShmEnableZeroCopy(zero_copy.getValue());
            ecal_imu_pub_vec.push_back(pub);
            auto queue = new std::queue<sensor_msgs::Imu::ConstPtr>();
            ros_imu_queue_map[ros_sub_topics[i]] = queue;
            ros::ros_helper::instance().subscribe<sensor_msgs::Imu>(
                ros_sub_topics[i], 10,
                (boost::function<void(const sensor_msgs::Imu::ConstPtr&)>)[queue](
                    const sensor_msgs::Imu::ConstPtr& msg) { queue->push(msg); });
            std::thread thread(ros_to_ecal_imu_send, std::ref(queue), std::ref(pub), runs.getValue());
            thread_vec.push_back(std::move(thread));
        }
    }

    while (ros::ok()) {
        // sleep 500 ms
        eCAL::Process::SleepMS(500);
        if (g_msg_num >= runs.getValue()) {
            break;
        }
    }

    for (auto& th : thread_vec) {
        if (th.joinable()) {
            th.join();
        }
    }

    stop_signal_pub.Send("stop");
    eCAL::Process::SleepMS(1000);
    stop_signal_pub.Send("stop");
    eCAL::Process::SleepMS(1000);

    // finalize eCAL API
    eCAL::Finalize();

    return (0);
}
